//
// Created by 86156 on 25-7-7.
//

#ifndef CONTROL_DRIVER_H
#define CONTROL_DRIVER_H
#include <stdint.h>

#include "motor_driver.h"

#define DEPTHOFFSET -55
#define VERSION_QUANTITY 255


#define MOVE_UP_STEP 80
#define MOVE_FORWARD_STEP (-90)
#define MOVE_SIDE_STEP	  (-110)
#define MOVE_SWERVE_STEP  50

#define MOTOR1_OFFSET_RATE 1.19
#define MOTOR2_OFFSET_RATE 1.0
#define MOTOR3_OFFSET_RATE 1.19
#define MOTOR4_OFFSET_RATE 1.0

#define MOTOR5_OFFSET_RATE 1.6
#define MOTOR6_OFFSET_RATE 1.6
#define MOTOR7_OFFSET_RATE 1.0
#define MOTOR8_OFFSET_RATE 1.0

#define RC_MODE_FLAG 0

//#define Debug_Mode


typedef struct
{
    float Target_Yaw;
    float Target_Depth;

    float Forward_Speed;
    float Side_Speed;
    float Depth_Speed;

    uint8_t PID_YAW_flag;
    uint8_t PID_ROL_flag;
    uint8_t PID_Depth_flag;
    uint8_t Yaw_View_flag;
    uint8_t Side_View_flag;
    uint8_t Enable_flag;
}CMD_TYPE;

typedef struct
{
    float rol;
    float pit;
    float yaw;
} FLOAT_Angle;

typedef struct
{
    float x;
    float y;
    float z;
}FLOAT_xyz;

typedef struct
{
    uint8_t type;
    int16_t x;
    int16_t y;
    uint8_t area;
}Orange_Message;


extern CMD_TYPE TEST_mode;

extern PID_t PID_ROL_Angle;
extern PID_t PID_YAW_Angle;
extern PID_t PID_PIT_Angle;
extern PID_t PID_View;

extern PID_t PID_ROL_Speed;
extern PID_t PID_PIT_Speed;
extern PID_t PID_YAW_Speed;

extern PID_t PID_Depth;

extern FLOAT_Angle Angle_Measure;
extern FLOAT_Angle AngleRate_Measure;
extern FLOAT_xyz   Gry_Measure;

extern Orange_Message Version_Message[VERSION_QUANTITY];


extern uint16_t Depth;
extern uint16_t Coordinate_X ;
extern uint16_t Coordinate_Y ;
extern int16_t X_Measure;
extern int16_t Y_Measure;

// extern float Motor1_PWM,Motor2_PWM,Motor3_PWM,Motor4_PWM,Motor5_PWM,Motor6_PWM,Motor7_PWM,Motor8_PWM;
extern Motor_t Motor1,Motor2,Motor3,Motor4,Motor5,Motor6,Motor7,Motor8;

void Motor_Cal(CMD_TYPE*CMD);
void Set_Motor_Speed(Motor_t motor);
void Motor_Pid(CMD_TYPE*CMD);
#endif //CONTROL_DRIVER_H
